/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : freertos.c
  * Description        : Code for freertos applications
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "can.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "oled.h"
#include "oled_data.h"
#include "Caculate.h"
#include "DJI.h"
#include "STP23.h"
#include	"wtr_can.h"
#include "delay_us.h"
#include "motor_mg513.h"
#include "math.h"
#include "delay_us.h"
#include "datou.h"
#include "Control.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
extern CAN_HandleTypeDef hcan1;
u8 jiaqu[2] = { 0,0 };
extern u16 distance,distance_y;
/*store_information数组表示的是库存中1到6号位置的货物的编号，为123456的一组排列，如132456，表示一号货架上有1号货物，3号货架上有2号货物，2号货架上有3号货物，4号货架上有4号货物，5号货架上有5号货物，6号货架上有6号货物*/
// u8 store_information[6]={4,6,5,2,1,3};
// /*place_information数组表示的是放置位置654321号位置的货物的编号，由123456中五个数字和一个7的一种排列，如712345，表示6号位置不放货物，5号位置放1号货物，4号位置放2号货物，3号位置放3号货物，2号位置放4号货物，1号位置放5号货物，缺少的6号货物随便叠放到一个放置位置上即可*/
// u8 place_information[6]={2,1,7,4,3,5};
// // 货物需求映射表：货物编号 -> 放置位置（0表示无位置）
extern u8 place_information[6];
extern u8 store_information[6];

u8 goods_to_place[7] = {0};

// 货架状态（0=未夹取，1=已夹取）
u8 picked[6] = {0};

// 放置位置状态（0=未放置，1=已放置）
u8 placed[7] = {0}; 
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
void execute_trip(int claw1_shelf, int claw2_shelf, int is_last_trip);
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */

/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
  .name = "defaultTask",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for myTask02 */
osThreadId_t myTask02Handle;
const osThreadAttr_t myTask02_attributes = {
  .name = "myTask02",
  .stack_size = 1024 * 4,
  .priority = (osPriority_t) osPriorityHigh,
};
/* Definitions for myTask03 */
osThreadId_t myTask03Handle;
const osThreadAttr_t myTask03_attributes = {
  .name = "myTask03",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
/* Definitions for myTask04 */
osThreadId_t myTask04Handle;
const osThreadAttr_t myTask04_attributes = {
  .name = "myTask04",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal1,
};

/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */

/* USER CODE END FunctionPrototypes */

void StartDefaultTask(void *argument);
void StartTask02(void *argument);
void StartTask03(void *argument);
void StartTask04(void *argument);

void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */

/**
  * @brief  FreeRTOS initialization
  * @param  None
  * @retval None
  */
void MX_FREERTOS_Init(void) {
  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Create the thread(s) */
  /* creation of defaultTask */
  defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);

  /* creation of myTask02 */
  myTask02Handle = osThreadNew(StartTask02, NULL, &myTask02_attributes);

  /* creation of myTask03 */
  myTask03Handle = osThreadNew(StartTask03, NULL, &myTask03_attributes);

  /* creation of myTask04 */
  myTask04Handle = osThreadNew(StartTask04, NULL, &myTask04_attributes);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_EVENTS */
  /* add events, ... */
  /* USER CODE END RTOS_EVENTS */

}

/* USER CODE BEGIN Header_StartDefaultTask */
/**
  * @brief  Function implementing the defaultTask thread.
  * @param  argument: Not used
  * @retval None
  */
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END StartDefaultTask */
}

/* USER CODE BEGIN Header_StartTask02 */
/**
* @brief Function implementing the myTask02 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask02 */
void StartTask02(void *argument) {
  /* USER CODE BEGIN StartTask02 */
  // 步骤1: 确定缺失货物 (123456中未在place_information出现的数字)
  u8 missing_goods = 0;
  for (u8 goods = 1; goods <= 6; goods++) {
    u8 found = 0;
    for (u8 i = 0; i < 6; i++) {
      if (place_information[i] == goods) {
        found = 1;
        break;
      }
    }
    if (!found) {
      missing_goods = goods;
      break;
    }
  }

  // 步骤2: 构建货物->目标位置的映射 (target_place[货物]=位置1-6, 0表示无位置)
  u8 target_place[7] = {0}; // 索引1-6有效
  for (u8 i = 0; i < 6; i++) {
    if (place_information[i] != 7) {
      target_place[place_information[i]] = 6 - i; // 位置转换: 索引0->位置6, 索引5->位置1
    }
  }

  // 步骤3: 确定关键货物 (P1=位置1货物, P2=位置2货物, P5=位置5货物, P6=位置6货物)
  u8 P1 = 0, P2 = 0, P5 = 0, P6 = 0;
  for (u8 goods = 1; goods <= 6; goods++) {
    if (target_place[goods] == 1) P1 = goods;
    if (target_place[goods] == 2) P2 = goods;
    if (target_place[goods] == 5) P5 = goods;
    if (target_place[goods] == 6) P6 = goods;
  }

  // 步骤4: 规划三趟操作
  u8 shelves[6]; // 货架1-6上的货物
  for (u8 i = 0; i < 6; i++) {
    shelves[i] = store_information[i];
  }
  
  // 初始化三趟夹取方案
  u8 trip_claw1[3] = {0}; // 每趟夹爪1夹取的货架
  u8 trip_claw2[3] = {0}; // 每趟夹爪2夹取的货架
  u8 trip_goods1[3] = {0}; // 每趟夹爪1夹取的货物
  u8 trip_goods2[3] = {0}; // 每趟夹爪2夹取的货物
  u8 visited[6] = {0};    // 货架访问标记 (0-5对应货架1-6)

  // 规划第一趟: 优先处理P2和P5
  // 夹爪1 (4-6号货架)
  for (u8 shelf = 3; shelf < 6; shelf++) { // 货架4-6 (索引3-5)
    if (visited[shelf]) continue;
    if (!target_place[shelf]) continue;
    u8 goods = shelves[shelf];
    if (goods == P2 || goods == P5) {
      trip_claw1[0] = shelf + 1;
      trip_goods1[0] = goods;
      visited[shelf] = 1;
      break;
    }
  }
  // 如果未找到P2/P5,
  if (!trip_claw1[0]) {
    for (u8 shelf = 3; shelf < 6; shelf++) {
      if (visited[shelf]) continue;
      if (!target_place[shelf]) continue;      
      u8 goods = shelves[shelf];
      if (goods == P1 || goods == P6) {
      trip_claw1[0] = shelf + 1;
      trip_goods1[0] = goods;
      visited[shelf] = 1;
      break;
    }   
    }
  }
  // 如果仍未找到, 选择任意货物
  if (!trip_claw1[0]) {
    for (u8 shelf = 3; shelf < 6; shelf++) {
      if (visited[shelf]) continue;
      if (!target_place[shelf]) continue;
      trip_claw1[0] = shelf + 1;
      trip_goods1[0] = shelves[shelf];
      visited[shelf] = 1;
      break;
    }
  }

  // 夹爪2 (1-3号货架)
  for (u8 shelf = 0; shelf < 3; shelf++) { // 货架1-3 (索引0-2)
    if (visited[shelf]) continue;
    u8 goods = shelves[shelf];  
    if (goods==missing_goods) continue;     
    
    if(trip_goods1[0] == P1) 
    {
      if(P2)
      {
        if (shelves[shelf] == P2) {
          trip_claw2[0] = shelf + 1;
          trip_goods2[0] = P2;
          visited[shelf] = 1;
          break;
        }
      }
    }     
    else if(trip_goods1[0] == P2) 
    {
      if(P1)
      {
        if (shelves[shelf] == P1) {
          trip_claw2[0] = shelf + 1;
          trip_goods2[0] = P1;
          visited[shelf] = 1;
          break;
        }
      }
    }   
    else if(trip_goods1[0] == P6) 
    {
      if(P5)
      {
        if (shelves[shelf] == P5) {
          trip_claw2[0] = shelf + 1;
          trip_goods2[0] = P5;
          visited[shelf] = 1;
          break;
        }
      }
    }  
    else if(trip_goods1[0] == P5) 
    {
      if(P6)
      {
        if (shelves[shelf] == P6) {
          trip_claw2[0] = shelf + 1;
          trip_goods2[0] = P6;
          visited[shelf] = 1;
          break;
        }
      }
    }      
    else if (goods == P2 || goods == P5)
    {
        trip_claw2[0] = shelf + 1;
        trip_goods2[0] = goods;
        visited[shelf] = 1;
        break;      
      }
    }
  if (!trip_claw2[0]) {
    for (u8 shelf = 0; shelf < 3; shelf++) {
      if (visited[shelf]) continue;
      if (!target_place[shelf]) continue;       
      u8 goods = shelves[shelf];
      if (goods != P1 && goods != P6) {
        trip_claw2[0] = shelf + 1;
        trip_goods2[0] = goods;
        visited[shelf] = 1;
        break;
      }
    }
  }
  if (!trip_claw2[0]) {
    for (u8 shelf = 0; shelf < 3; shelf++) {
      if (visited[shelf]) continue;
      trip_claw2[0] = shelf + 1;
      trip_goods2[0] = shelves[shelf];
      visited[shelf] = 1;
      break;
    }
  }

  // 规划第二趟: 处理剩余货物(除缺失货物外)
  // 夹爪1 (4-6号货架)
  for (u8 shelf = 3; shelf < 6; shelf++) { // 货架4-6 (索引3-5)
    if (visited[shelf]) continue;
    u8 goods = shelves[shelf];
    if (goods==missing_goods) continue;       
    if (goods == P2 || goods == P5) {
      trip_claw1[1] = shelf + 1;
      trip_goods1[1] = goods;
      visited[shelf] = 1;
      break;
    }
  }
  // 如果未找到P2/P5,
  if (!trip_claw1[1]) {
    for (u8 shelf = 3; shelf < 6; shelf++) {
      if (visited[shelf]) continue;     
      u8 goods = shelves[shelf];
      if (goods==missing_goods) continue;         
    if (goods == P1 || goods == P6 ) {
      trip_claw1[1] = shelf + 1;
      trip_goods1[1] = goods;
      visited[shelf] = 1;
      break;
    }
    }
  }
  // 如果仍未找到, 选择任意货物
  if (!trip_claw1[1]) {
    for (u8 shelf = 3; shelf < 6; shelf++) {
      if (visited[shelf]) continue;
      u8 goods = shelves[shelf];
      if (goods==missing_goods) continue;  
      trip_claw1[1] = shelf + 1;
      trip_goods1[1] = shelves[shelf];
      visited[shelf] = 1;
      break;
    }
  }

  // 夹爪2 (1-3号货架)
  for (u8 shelf = 0; shelf < 3; shelf++) { // 货架1-3 (索引0-2)
    if (visited[shelf]) continue;
    u8 goods = shelves[shelf];   
    if (goods==missing_goods) continue;       
    if(trip_goods1[1] == P1) 
    {
      if(P2)
      {
        if (shelves[shelf] == P2) {
          trip_claw2[1] = shelf + 1;
          trip_goods2[1] = P2;
          visited[shelf] = 1;
          break;
        }
      }
    }     
    else if(trip_goods1[1] == P2) 
    {
      if(P1)
      {
        if (shelves[shelf] == P1) {
          trip_claw2[1] = shelf + 1;
          trip_goods2[1] = P1;
          visited[shelf] = 1;
          break;
        }
      }
    }   
    else if(trip_goods1[1] == P6) 
    {
      if(P5)
      {
        if (shelves[shelf] == P5) {
          trip_claw2[1] = shelf + 1;
          trip_goods2[1] = P5;
          visited[shelf] = 1;
          break;
        }
      }
    }  
    else if(trip_goods1[1] == P5) 
    {
      if(P6)
      {
        if (shelves[shelf] == P6) {
          trip_claw2[1] = shelf + 1;
          trip_goods2[1] = P6;
          visited[shelf] = 1;
          break;
        }
      }
    }      
    else if (goods == P2 || goods == P5)
    {
        trip_claw2[1] = shelf + 1;
        trip_goods2[1] = goods;
        visited[shelf] = 1;
        break;      
      }
    }
  if (!trip_claw2[1]) {
    for (u8 shelf = 0; shelf < 3; shelf++) {
      if (visited[shelf]) continue;    
      u8 goods = shelves[shelf];
      if (goods==missing_goods) continue;        
      if (goods != P1 && goods != P6) {
        trip_claw2[1] = shelf + 1;
        trip_goods2[1] = goods;
        visited[shelf] = 1;
        break;
      }
    }
  }
  if (!trip_claw2[1]) {
    for (u8 shelf = 0; shelf < 3; shelf++) {
      if (visited[shelf]) continue;
      trip_claw2[1] = shelf + 1;
      trip_goods2[1] = shelves[shelf];
      visited[shelf] = 1;
      break;
    }
  }


  // 规划第三趟: 处理剩余两个货架(包含缺失货物)
  for (u8 shelf = 0; shelf < 6; shelf++) {
    if (!visited[shelf]) {
      if (!trip_claw2[2]) {
        trip_claw2[2] = shelf + 1;
        trip_goods2[2] = shelves[shelf];
        visited[shelf] = 1;
      } else if (!trip_claw1[2]) {
        trip_claw1[2] = shelf + 1;
        trip_goods1[2] = shelves[shelf];
        visited[shelf] = 1;        
      }
    }
  }

  // 步骤5: 执行三趟操作
  u8 P2_placed = 0, P5_placed = 0; // 关键位置放置状态
  
  for (u8 trip = 0; trip < 3; trip++) {
    // 夹取货物
    claw_store_control(trip_claw1[trip], 1);
    claw_store_control(trip_claw2[trip], 2);
    
    // 移动到中转区域
    xyztheta_control(X4_placement_centre, Y3_placement_centre, z_placement_3, 540,
                     &hDJI[0], &hDJI[1], &hDJI[3], &hDJI[4], &hcan1);
    
    if (trip < 2) { // 第一、二趟正常放置
      u8 goods1 = trip_goods1[trip];
      u8 goods2 = trip_goods2[trip];
      u8 place1 = target_place[goods1];
      u8 place2 = target_place[goods2];
      
      // 处理关键位置放置顺序
      if (!P2_placed && goods2 == P2) { // 先放P2
        claw_place_control(2, 2);
        P2_placed = 1;
        // 再放另一个货物
        if (goods1 == P1) {
          claw_place_control(1, 1);
        } else {
          claw_place_control(place1, 1);
        }
      }
      else if (!P5_placed && goods1 == P5) { // 先放P5
        claw_place_control(5, 1);
        P5_placed = 1;
        // 再放另一个货物
        if (goods2 == P6) {
          claw_place_control(6, 2);
        } else {
          claw_place_control(place2, 2);
        }
      }
      else { // 默认顺序放置
        claw_place_control(place1, 1);
        claw_place_control(place2, 2);
        
        // 更新关键位置状态
        if (goods1 == P2 || goods2 == P2) P2_placed = 1;
        if (goods1 == P5 || goods2 == P5) P5_placed = 1;
      }
    } 
    else { // 第三趟叠放处理
      u8 goods1 = trip_goods1[2];
      u8 goods2 = trip_goods2[2];
      u8 stack_pos = 0;
      
      // 确定叠放位置
      if (goods1 == missing_goods) {
        stack_pos = target_place[goods2];
      } else if (goods2 == missing_goods) {
        stack_pos = target_place[goods1];
      } else {
        // 理论上不会发生, 默认使用货物1的位置
        stack_pos = target_place[goods1];
      }
      
      claw_stacked_control(stack_pos);
    }
    
    // 返回中转区域
    xyztheta_control(X4_placement_centre, Y3_placement_centre, z_placement_3, 0,
                     &hDJI[0], &hDJI[1], &hDJI[3], &hDJI[4], &hcan1);
  }
  
  // 结束任务
  xyztheta_control(600, 600, z_placement_3, 0,
                   &hDJI[0], &hDJI[1], &hDJI[3], &hDJI[4], &hcan1);
  
  /* Infinite loop */
  for (;;) {
    OLED_Clear();
    OLED_ShowString(1, 1, "Task is over", OLED_8X16);
    OLED_Update();
    osDelay(1);
  }
  /* USER CODE END StartTask02 */
}

/* USER CODE BEGIN Header_StartTask03 */
/**
* @brief Function implementing the myTask03 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask03 */
void StartTask03(void *argument)
{
  /* USER CODE BEGIN StartTask03 */
  /* Infinite loop */
  for(;;)
  {
    if (jiaqu[0] == 1)
    {
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0);
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 1);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 998);
        osDelay(5);
        HAL_GPIO_WritePin(AIN1_1_GPIO_Port, AIN1_1_Pin, 0);
        HAL_GPIO_WritePin(AIN2_1_GPIO_Port, AIN2_1_Pin, 0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
    }
    if (jiaqu[1] == 1)
    {
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0);
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 1);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 998);
        osDelay(5);
        HAL_GPIO_WritePin(AIN1_2_GPIO_Port, AIN1_2_Pin, 0);
        HAL_GPIO_WritePin(AIN2_2_GPIO_Port, AIN2_2_Pin, 0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);

    }

    osDelay(1);
  }
  /* USER CODE END StartTask03 */
}

/* USER CODE BEGIN Header_StartTask04 */
/**
* @brief Function implementing the myTask04 thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartTask04 */
void StartTask04(void *argument)
{
  /* USER CODE BEGIN StartTask04 */
  /* Infinite loop */
  for(;;)
  {
    osDelay(1);
  }
  /* USER CODE END StartTask04 */
}

/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void execute_trip(int claw1_shelf, int claw2_shelf, int is_last_trip) {
  // 获取货物信息
  int good1 = store_information[claw1_shelf];
  int good2 = store_information[claw2_shelf];
  int place1 = goods_to_place[good1];
  int place2 = goods_to_place[good2];
  
  // 标记货架已夹取
  picked[claw1_shelf] = 1;
  picked[claw2_shelf] = 1;
  
  // 夹取货物
  claw_store_control(claw1_shelf + 1, 1);  // 货架号=索引+1
  claw_store_control(claw2_shelf + 1, 2);
  
  // 移动到中转区
  xyztheta_control(X4_placement_centre, Y3_placement_centre, 
                   z_placement_3, 540, &hDJI[0], &hDJI[1], 
                   &hDJI[3], &hDJI[4], &hcan1);
  
  // 放置货物（最后一趟特殊处理）
  if (!is_last_trip) {
    // 优先放置位置2/5的货物
    if (place1 == 2 || place1 == 5) {
      claw_place_control(place1, 1);
      placed[place1] = 1;
      claw_place_control(place2, 2);
      placed[place2] = 1;
    } else if (place2 == 2 || place2 == 5) {
      claw_place_control(place2, 2);
      placed[place2] = 1;
      claw_place_control(place1, 1);
      placed[place1] = 1;
    } else {
      // 普通放置
      claw_place_control(place1, 1);
      placed[place1] = 1;
      claw_place_control(place2, 2);
      placed[place2] = 1;
    }
  } else {
    // 最后一趟：一个正常放置，一个叠放
    if (place1 != 0) {
      placed[place1] = 1;
      claw_stacked_control(place1);  // 叠放到相同位置
    } else {
      placed[place2] = 1;
      claw_stacked_control(place2);
    }
  }
  
  // 返回中转区
  xyztheta_control(X4_placement_centre, Y3_placement_centre, 
                   z_placement_3, 0, &hDJI[0], &hDJI[1], 
                   &hDJI[3], &hDJI[4], &hcan1);
}
/* USER CODE END Application */